/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package teamBlitz;

import com.sun.squawk.security.HexEncoding;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SensorBase;
import edu.wpi.first.wpilibj.Timer;
import java.util.TimerTask;

/**
 *
 * @author dtjones
 */




public class HMC6352 extends I2CSync {

private static final byte BASE_ADDR = (byte)0x42; //MAG address on i2c bus
private static final byte CNTRL2 = 0x11;              // cntrl register2
private static final byte CNTRL1 = 0x10;
private static final byte XLOW = 0x01;              //Y?

    private java.util.Timer m_controlLoop;
    //private final DigitalModule module;
    private I2C m_i2c;
    int m_Count;
    float m_Total;
 

    public static final class Direction{
        public static final int CW=1;
        public static final int CCW=-1;
        public static final int STOPPED=0;
    }
    

    /**
     * Constructor.
     *
     * @param slot The slot of the digital module that the sensor is plugged into.
     * @param range The range (+ or -) that the accelerometer will measure.
     */
    float m_heading;
    private int m_Direction=Direction.STOPPED;

    public int getDirection() {
        return m_Direction;
    }

    public float getHeading() {
        return m_heading/10;
    }
      public float getAverageHeading(){
          if(m_Count!=0){
          float AverageHeading= (m_Total/m_Count)/10;
          m_Count=0;
          m_Total=0;
          return AverageHeading;
                  
          }
          return getHeading();
      }
    
    public HMC6352(int slot) throws InstantiationException {
        super(DigitalModule.getInstance(slot),BASE_ADDR);
        m_Total=0;
        m_Count=0;
        boolean rc=false;
        System.out.println("teamblitz HMC6352 started");
        byte[] outCmd=new byte[4];
        outCmd[0]='g';  // command to write to ram
        outCmd[1]=0x74; // ram location of operations register
        outCmd[2]=0x42; // 20Hz operaion continuous.
        rc=transaction(outCmd,3,null,0);
        if(rc==true) // transcation aborted
        {
             System.out.println("Did not connect to compass");
            throw(new InstantiationException() );
  
        }

        m_controlLoop = new java.util.Timer();

        m_controlLoop.schedule(new ChkTask(this), 0L, (long) (100));

    }
    
    public boolean update() {
        boolean aborted;
        float oldHeading=m_heading;
        byte[] input = new byte[2];        
        byte[] rawData = new byte[6];
        aborted=read(0x41, 2, rawData);
        
        if(!aborted){
            m_heading = (float)(((short)rawData[0]<<8)|((short)rawData[1]&0x00ff));

            // check for direction of movement
            // make assumtion that a robot cannot turn a 
            // full 180 degrees in 100 ms
            if(m_heading==oldHeading){
                m_Direction=Direction.STOPPED;
            }else if(oldHeading>1800){  
                if((m_heading>(oldHeading-1800))&&(m_heading<oldHeading)){
                    m_Direction=Direction.CCW;
                }else {
                    m_Direction=Direction.CW;
                }
           }else{  
                if((m_heading<(oldHeading+1800))&&(m_heading<oldHeading)){
                    m_Direction=Direction.CW;
                }else {
                    m_Direction=Direction.CCW;
                }
            }           
            
        }
        m_Total=m_Total+m_heading;
        m_Count++;
        //System.out.println("compass="+HexEncoding.hexEncode(rawData,2)+"="+m_heading);
 
        return true;
   }
    
     private class ChkTask extends TimerTask {

        private HMC6352 m_me;

        public ChkTask(HMC6352 me) {
            if (me == null) {
                throw new NullPointerException("Given HMC6352 was null");
            }
            m_me = me;
        }

        public void run() {
            m_me.update();
        }
    }
}
